
#include "main.h"
#include "uart.h"


int distance_arr[3];
int fd; //uart file handle

int main()
{
	int res = 0;
	int n_read;
	char r_buff[128] = {'\0'};
	int recv= 0;
	
	uint8_t type;
	//init wiringPi
	if(-1 == wiringPiSetup()){
			printf("Set up fail\n");
			exit(-1);
	}
	
	fd = serialOpen("/dev/ttyAMA0",115200);
	if(-1 == fd){
		printf("Open serial fail\n");
		exit(-1);
	}
	
	
	printf("start\r\n");
	serialFlush(fd);
	
	type = DIST_CMD_TYPE;
	while(1){
		//发送请求
		serialPrintf(fd,"AA%cDD",(char)type);
		//
		res = recv_respond(fd, type);
		
		if(res != 0){
			printf("result:%d\n",res);
		}
		else{
			printf("dist_an0:%d\t dist_an1:%d\t dist_an2:%d\n",distance_arr[0],distance_arr[1],distance_arr[2]);
			
		}
		sleep(1);
		
		
		
		
		
	}

	return 0;
}




// int main()
// {
	// vec3d	anchorpos[3];
	// vec3d solution;

	// anchorpos[0].x = 0.30;
	// anchorpos[0].y = 0;
	// anchorpos[0].z = 0;
	// anchorpos[1].x = 0.15;
	// anchorpos[1].y = 0.30;
	// anchorpos[1].z = 0;
	// anchorpos[2].x = 0;
	// anchorpos[2].y = 0;
	// anchorpos[2].z = 0;

	// int distancelist[3] = { 140,240,160 };

	// GetLocation2(&solution, 0 , &anchorpos[0], &distancelist[0]);
	// printf("%lf,%lf\r\n",solution.x,solution.y);
	
    // return 0;
// }